/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       gimbal_task.c/h
  * @brief      gimbal control task, because use the euler angle calculated by
  *             gyro sensor, range (-pi,pi), angle set-point must be in this
  *             range.gimbal has two control mode, gyro mode and enconde mode
  *             gyro mode: use euler angle to control, encond mode: use enconde
  *             angle to control. and has some special mode:cali mode, motionless
  *             mode.
  *             完成云台控制任务，由于云台使用陀螺仪解算出的角度，其范围在（-pi,pi）
  *             故而设置目标角度均为范围，存在许多对角度计算的函数。云台主要分为2种
  *             状态，陀螺仪控制状态是利用板载陀螺仪解算的姿态角进行控制，编码器控制
  *             状态是通过电机反馈的编码值控制的校准，此外还有校准状态，停止状态等。
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. add some annotation
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "gimbal_task.h"

#include "CAN_communication.h"
#include "INS_task.h"
#include "cmsis_os.h"
#include "custom_controller_connect.h"
#include "main.h"
#include "usb_task.h"

typedef struct __IMU
{
    float yaw, pitch, roll;           // rad
    float yawSpd, pitchSpd, rollSpd;  // rad/s
    float xAccel, yAccel, zAccel;     // m/s^2
} IMU_t;

typedef __packed struct __PackedPostureInfo{
    float yaw;
    float big_arm_pitch;
    float small_arm_pitch;
    float small_arm_roll;
} PackedPostureInfo_t;


IMU_t IMU;

uint32_t TICK_CNT = 0;

/**
 * @brief          云台任务，间隔 GIMBAL_CONTROL_TIME 1ms
 * @param[in]      pvParameters: 空
 * @retval         none
 */
void gimbal_task(void const * pvParameters)
{
    // wait a time
    vTaskDelay(GIMBAL_TASK_INIT_TIME);

    while (1) {
        //获取姿态
        IMU.yaw = get_INS_angle_point()[0];
        IMU.pitch = get_INS_angle_point()[1];
        IMU.roll = get_INS_angle_point()[2];

        IMU.yawSpd = get_gyro_data_point()[2];
        IMU.pitchSpd = get_gyro_data_point()[1];
        IMU.rollSpd = get_gyro_data_point()[0];

        IMU.xAccel = get_accel_data_point()[0];
        IMU.yAccel = get_accel_data_point()[1];
        IMU.zAccel = get_accel_data_point()[2];

#if (SELF_C_BOARD == C_BOARD_MAIN_ID)
        PostureInfo_t * posture_info = GetPostureInfoPoint();

        OutputPCData.data_1 = posture_info->yaw;
        OutputPCData.data_2 = posture_info->big_arm_pitch;
        OutputPCData.data_3 = posture_info->small_arm_pitch;
        OutputPCData.data_4 = posture_info->small_arm_roll;
        
        // 发送数据到电脑
        PackedPostureInfo_t packed_posture_info;
        packed_posture_info.yaw = posture_info->yaw;
        packed_posture_info.big_arm_pitch = posture_info->big_arm_pitch;
        packed_posture_info.small_arm_pitch = posture_info->small_arm_pitch;
        packed_posture_info.small_arm_roll = posture_info->small_arm_roll;
        
        if (TICK_CNT >= 10) {
            TICK_CNT = 0;
            SendDataToPC((uint8_t *)(&packed_posture_info));
        } else {
            TICK_CNT++;
        }

#elif (SELF_C_BOARD == C_BOARD_BIG_ARM_ID)
        // 解算控制量
        CanSendData(BIG_ARM_ID, IMU.yaw, IMU.pitch);
#elif (SELF_C_BOARD == C_BOARD_SMALL_ARM_ID)
        // 解算控制量
        CanSendData(SMALL_ARM_ID, IMU.roll, IMU.pitch);
#elif (SELF_C_BOARD == C_BOARD_3_ID)
#endif

        vTaskDelay(GIMBAL_CONTROL_TIME);
    }
}
